// DRV8833Controller.cpp
#include "DRV8833Controller.h"
#include <Arduino.h>

DRV8833Controller::DRV8833Controller(int forward_pin, int backward_pin, int turn_left_pin, int turn_right_pin) {
  _forward_pin = forward_pin;
  _backward_pin = backward_pin;
  _turn_left_pin = turn_left_pin;
  _turn_right_pin = turn_right_pin;

  pinMode(_forward_pin, OUTPUT);
  pinMode(_backward_pin, OUTPUT);
  pinMode(_turn_left_pin, OUTPUT);
  pinMode(_turn_right_pin, OUTPUT);

  // Setting PWM properties
  const int freq = 30000;
  const int resolution = 8;
  // configure LED PWM functionalitites
  ledcAttach(_forward_pin, freq, resolution);
  ledcAttach(_backward_pin, freq, resolution);

  ledcAttach(_turn_left_pin, freq, resolution);
  ledcAttach(_turn_right_pin, freq, resolution);

  // robot running state
  state = ROBOT_STOP;
  cur_robot_cmd = ROBOT_STOP;

  // PWM - speed
  speed = 220;
  turn_speed = 255;
  speed_step = 10;

  stop();  // 初始状态停止
  center();
}

void DRV8833Controller::forward() {
  ledcWrite(_forward_pin, speed);
  ledcWrite(_backward_pin, 0);
}

void DRV8833Controller::backward() {
  ledcWrite(_forward_pin, 0);
  ledcWrite(_backward_pin, speed);
}

void DRV8833Controller::stop() {
  ledcWrite(_forward_pin, 0);
  ledcWrite(_backward_pin, 0);
}

void DRV8833Controller::turn_left() {
  ledcWrite(_turn_left_pin, turn_speed);
  ledcWrite(_turn_right_pin, 0);
}

void DRV8833Controller::turn_right() {
  ledcWrite(_turn_left_pin, 0);
  ledcWrite(_turn_right_pin, turn_speed);
}

void DRV8833Controller::center() {
  //  简易阿克曼转向(ackermann  steering)。转向电机停止，转向的机械结构自动居中
  ledcWrite(_turn_left_pin, 0);
  ledcWrite(_turn_right_pin, 0);
}

void DRV8833Controller::_running_with_new_speed() {
  switch (state) {
    case ROBOT_FORWARD:
      forward();
      break;
    case ROBOT_BACKWARD:
      backward();
      break;
    default:
      break;
  }
}

void DRV8833Controller::speed_up() {
  speed += speed_step;
  if (speed > 255) {
    speed = 255;
  }
  _running_with_new_speed();
}

void DRV8833Controller::speed_down() {
  speed -= speed_step;
  if (speed < 0) {
    speed = 0;
  }
  _running_with_new_speed();
}

void DRV8833Controller::action(ROBOT_ACTION_CODE robot_cmd) {
  switch (robot_cmd) {
    case ROBOT_FORWARD:
      forward();
      state = ROBOT_FORWARD;
      break;
    case ROBOT_BACKWARD:
      backward();
      state = ROBOT_BACKWARD;
      break;
    case ROBOT_STOP:
      stop();
      state = ROBOT_STOP;
      break;
    case ROBOT_TURN_LEFT:
      turn_left();
      break;
    case ROBOT_TURN_RIGHT:
      turn_right();
      break;
    case ROBOT_CENTER:
      center();
      break;
    case ROBOT_SPEED_UP:
      speed_up();
      break;
    case ROBOT_SPEED_DOWN:
      speed_down();
      break;
    default:
      break;
  }  // switch
};
